#!/usr/bin/env python
#coding:utf-8

import DLRS1A
import Intermec
import RTControl as RTC
from RTControl import Outputor
from Debug import debug, error
import Globals
import Model
import numpy as np
import Database
from math import *
from datetime import datetime
import Queue
import Atlas
import time
import re
        
class SensorCollector(RTC.CyclicCollector):
    def __init__(self, domain):
        RTC.CyclicCollector.__init__(self, 0.2)
        self.domain = domain
    
    def OneCyclic(self):
        self.domain.UpdateSensorData()

class PWDomain(object):
    def __init__(self):
        self.con = None
        self.pvs = None
        self.cap_result = False
        self.error_flag = False
        self.ready = False
        self.angle_result = Queue.Queue()
        self.barcodequeue = Queue.Queue()
        self.soqueue = Queue.Queue()
        self.foc_result = []
        
        self.okcount = 0
        self.nokcount = 0
        self.firsttime = None
        self.lasttime = None
        
        self.part_type = ''
        self.last_so_parttype = None
        self.part_barcode = ''
        self.last_barcode = None
        self.begin_time = None
        self.end_time = None
        self.last_so_result = None
        self.part_result = False
        self.unready_sensor_flag = False
        
        self.min_lcambangle = -100.0
        self.max_lcambangle = 100.0
        self.min_lconsangle = -100.0
        self.max_lconsangle = 100.0
        self.min_rcambangle = -100.0
        self.max_rcambangle = 100.0
        self.min_rconsangle = -100.0
        self.max_rconsangle = 100.0
        
        try:
            self.rs1a = DLRS1A.DLRS1A(6, 'COM1', 9600, 8, 'N', 1)
            app.rs1a = self.rs1a
            self.intermec = Intermec.SerialIntermec('COM2', 9600, 8, 'N', 1)
            self.intermec.CheckData = self.OnScanBarcode
            self.foc = Atlas.Foc('Foc1', '192.168.0.35', 4545)
            self.foc.Listener = self.OnFocResult
        except:
            self.rsia = None
            self.intermec = None
            self.foc = None
            error.traceback()
        
    def GetReadyFlag(self):
        return self.ready
    
    def SetControl(self, cond):
        self.con = cond
        
    def SetPartType(self, parttype):
        try:
            r = Model.MPartTypes.Find(parttype)
        except:
            r = None
        if not r:
            self.min_lconsangle = -100.0
            self.max_lconsangle = 100.0
            self.min_lcambangle = -100.0
            self.max_lcambangle = 100.0
            self.min_rconsangle = -100.0
            self.max_rconsangle = 100.0
            self.min_rcambangle = -100.0
            self.max_rcambangle = 100.0
            self.part_barcode = ''
        else:
            self.min_lconsangle = r.lstdconsangle
            self.max_lconsangle = r.lconsangletoler
            self.min_lcambangle = r.lstdcambangle
            self.max_lcambangle = r.lcambangletoler
            self.min_rconsangle = r.rstdconsangle
            self.max_rconsangle = r.rconsangletoler
            self.min_rcambangle = r.rstdcambangle
            self.max_rcambangle = r.rcambangletoler
            self.part_barcode = r.barcode
        self.last_so_parttype = r
        self.part_type = parttype
        self.con.UpdateTolerData()
        
    def GetPartType(self):
        return self.part_type
    
    def GetTolerData(self):
        try:
            r = Model.MPartTypes.Find(self.part_type)
        except:
            r = None
        if not r:
            return None,None,None,None,None,None,None,None
        return r.lstdconsangle, r.lconsangletoler, r.lstdcambangle, r.lcambangletoler, r.rstdconsangle, r.rconsangletoler, r.rstdcambangle, r.rcambangletoler
        
    def BeginWork(self):
        self.sensorcollector = SensorCollector(self)
        
        self.sensorcollector.Start()
        if self.intermec:
            self.intermec.Start()
        if self.foc:
            self.foc.Connect()
    
    def EndWork(self):
        self.sensorcollector.Stop()
        if self.intermec:
            self.intermec.Stop()
        if self.foc:
            self.foc.Disconnect()
            
    def UpdateSensorData(self):
        try:
            self.pvs = self.rs1a.M0()
            if len(self.pvs) < 6:
                self.pvs = self.rs1a.M0()
            try:
                self.pvs[0] += app.pref.left_pv1_offset
                self.pvs[1] += app.pref.left_pv2_offset
                self.pvs[2] += app.pref.left_pv3_offset
                self.pvs[3] += app.pref.right_pv4_offset
                self.pvs[4] += app.pref.right_pv5_offset
                self.pvs[5] += app.pref.right_pv6_offset
            except:
                pass
            if self.ready:
                self.__CheckPVBuffer(self.pvs)
                
            self.con.UpdateSensorData(self.pvs[0], self.pvs[1], self.pvs[2], self.pvs[3],
                                      self.pvs[4], self.pvs[5])
        except:
            error.traceback()
            
    def GetDistance(self):
        y1 = app.pref.left_y1
        x2 = app.pref.left_x2
        x3 = app.pref.left_x3
        y3 = app.pref.left_y3
        y4 = app.pref.right_y4
        x5 = app.pref.right_x5
        x6 = app.pref.right_x6
        y6 = app.pref.right_y6
        return y1, x2, x3, y3, y4, x5, x6, y6
    
    def GetCoorPoints(self):
        y1, x2, x3, y3, y4, x5, x6, y6 = self.GetDistance()
        return (0.0,y1),(x2,0.0),(x3,y3),(0.0,y4),(x5,0.0),(x6,y6)
            
    def GetLeftZeroPV(self):
        return Globals.pref.left_zero_pv1, Globals.pref.left_zero_pv2, Globals.pref.left_zero_pv3
    
    def GetRightZeroPV(self):
        return Globals.pref.right_zero_pv4, Globals.pref.right_zero_pv5, Globals.pref.right_zero_pv6
    
    def __CheckPVBuffer(self, pvs):
        y1, x2, x3, y3, y4, x5, x6, y6 = self.GetDistance()
        apv1, apv2, apv3, apv4, apv5, apv6 = pvs
        
        zpv1, zpv2, zpv3 = self.GetLeftZeroPV()
        zpv4, zpv5, zpv6 = self.GetRightZeroPV()
        
        l_cam_angle,l_con_angle,l_z = CalAngle(y1, x2, x3, y3, zpv1, apv1, zpv2, apv2, zpv3, apv3)
        r_cam_angle,r_con_angle,r_z = CalAngle(y4, x5, x6, y6, zpv4, apv4, zpv5, apv5, zpv6, apv6)
        
        l_cam_result = l_cam_angle <= self.max_lcambangle and l_cam_angle >= self.min_lcambangle
        l_con_result = l_con_angle <= self.max_lconsangle and l_con_angle >= self.min_lconsangle
        r_cam_result = r_cam_angle <= self.max_rcambangle and r_cam_angle >= self.min_rcambangle
        r_con_result = r_con_angle <= self.max_rconsangle and r_con_angle >= self.min_rconsangle
        
        if self.cap_result:
            self.angle_result.put_nowait([(l_cam_angle,l_cam_result),(l_con_angle,l_con_result),(r_cam_angle,r_cam_result),(r_con_angle,r_con_result)])
        
        self.con.UpdatePartPV(apv1, apv2, apv3, apv4, apv5, apv6)
        self.con.UpdateLeftResult(l_con_angle, l_con_result, l_cam_angle, l_cam_result)
        self.con.UpdateRightResult(r_con_angle, r_con_result, r_cam_angle, r_cam_result)
    
    def __GetPartCode(self):
        try:
            return self.barcodequeue.get_nowait()
        except:
            return None
        
    def __GetPartSO(self):
        return self.last_so
        if len(self.soqueue.queue) < 1:
            return None
        return self.soqueue.queue[0]
        
    def OnScanBarcode(self, data):
        try:
            data = data.split('\r')[0]
        except:
            self.con.ShowBusyMessage(u'条码数据存在错误')
            return
        
        typecode = self.GetTypeCode(data)
        if self.part_type != None and self.last_so_parttype.barcode != typecode:
            app.condomain.Red()
            self.con.ShowBusyMessage(u'条码型号与设置型号不匹配')
            self.con.UpdateBarcode(data, False)
            return
        
        typerror = False
        if app.pref.enable_type_sensor:
            typno = app.condomain.GetSensorType()
            if typno==2 and not typecode in Globals.high_types:
                typerror = True
            elif typno==1 and not typecode in Globals.mid_types:
                typerror = True
            elif typno==0 and not typecode in Globals.low_types:
                typerror = True
            elif typno==-1:
                typerror = True
            
        if typerror:
            app.condomain.Red()
            self.con.ShowBusyMessage(u'条码型号与实际工件的特征不匹配')
            self.con.UpdateBarcode(data, False)
            return
        
        self.last_barcode = data
        
        self.con.UpdateBarcode(data, True)
        
        if app.pref.no_barcode:
            return
        
        self.Sequnce()
        
    def GetTypeCode(self, barcode):
        rsear = re.search(app.pref.re_barcode_type, barcode)
        if not rsear:
            return None
        
        for v in rsear.groups():
            if v != None:
                return rsear.group(1)
            
        return None
    
    def GetPartCode(self, barcode):
        return barcode
                
    def OnFocResult(self, res):
        t, a, ok = res
        if ok == 'OK':
            if len(self.foc_result) < 4:
                self.foc_result.append(res)
            else:
                self.foc_result.pop(0)
                self.foc_result.append(res)
        else:
            self.con.ShowBusyMessage(u"拧紧不合格，扭矩：%.3f 角度：%.3f" % (t, a))
        if self.error_flag:
            self.error_flag = False
        
        self.con.UpdateFocResult(t, a, ok)
        
    def ClearCount(self):
        self.okcount = 0
        self.nokcount = 0
        self.firsttime = None
        self.lasttime = None
        
    def GetCountInfo(self):
        return self.okcount, self.nokcount, self.firsttime, self.lasttime
        
    def PartReady(self):
        self.ready = True
        self.error_flag = False
        self.begin_time = datetime.now()
        debug.info(u"工件放置就绪开始测试")
        self.con.OnPartReady()
        
    def PartUnReady(self):
        if self.error_flag:
            self.con.ShowBusyMessage(u'请等待数据处理中...', timeout=2, warn = False)
            l_cam_angle, l_cam_result, l_con_angle, l_con_result, r_cam_angle, r_cam_result, r_con_angle, r_con_result = self.GetAngle()
        
            self.ready = False
            self.error_flag = False
            
            self.end_time = datetime.now()
            self.lasttime =  self.end_time
            if self.okcount + self.nokcount == 0:
                self.firsttime = self.lasttime
                    
            if l_cam_result and l_con_result and r_cam_result and r_con_result and len(self.foc_result) == 2:
                self.okcount += 1
                self.part_result = True
                app.condomain.Green()
                self.con.UpdateResult(True)
            else:
                self.nokcount += 1
                self.part_result = False
                app.condomain.Red()
                self.con.UpdateResult(False)
                
            try:
                torque1, angle1, result1 = self.foc_result[0]
            except:
                torque1, angle1, result1 = None, None, None
                
            try:
                torque2, angle2, result2 = self.foc_result[1]
            except:
                torque2, angle2, result2 = None, None, None
                
            try:
                torque3, angle3, result3 = self.foc_result[2]
            except:
                torque3, angle3, result3 = None, None, None
                
            try:
                torque4, angle4, result4 = self.foc_result[3]
            except:
                torque4, angle4, result4 = None, None, None
                
            self.SaveResult(l_cam_angle, l_cam_result, l_con_angle, l_con_result, r_cam_angle, r_cam_result, r_con_angle, r_con_result, torque1, angle1, result1, torque2, angle2, result2, torque3, angle3, result3, torque4, angle4, result4)
        else:
            if len(self.foc_result) < 4:
                self.error_flag = True
                raise Exception(u'拧紧合格数小于4，请确认螺丝已经拧紧')
            
            self.con.ShowBusyMessage(u'请等待数据处理中...', timeout=2, warn = False)
            l_cam_angle, l_cam_result, l_con_angle, l_con_result, r_cam_angle, r_cam_result, r_con_angle, r_con_result = self.GetAngle()
        
            if l_cam_result and l_con_result and r_cam_result and r_con_result:
                self.ready = False
            else:
                self.error_flag = True
                raise Exception(u'角度测量不合格')
            
            self.end_time = datetime.now()
        
            self.lasttime =  self.end_time
            if self.okcount + self.nokcount == 0:
                self.firsttime = self.lasttime
            self.okcount += 1
            self.part_result = True
            app.condomain.Green()
            self.con.UpdateResult(True)
                
            try:
                torque1, angle1, result1 = self.foc_result[0]
            except:
                torque1, angle1, result1 = None, None, None
                
            try:
                torque2, angle2, result2 = self.foc_result[1]
            except:
                torque2, angle2, result2 = None, None, None
                
            try:
                torque3, angle3, result3 = self.foc_result[2]
            except:
                torque3, angle3, result3 = None, None, None
                
            try:
                torque4, angle4, result4 = self.foc_result[3]
            except:
                torque4, angle4, result4 = None, None, None
                
            self.SaveResult(l_cam_angle, l_cam_result, l_con_angle, l_con_result, r_cam_angle, r_cam_result, r_con_angle, r_con_result, torque1, angle1, result1, torque2, angle2, result2, torque3, angle3, result3, torque4, angle4, result4)
        
        self.last_barcode = None
            
    def GetAngle(self):
        self.cap_result = True
        time.sleep(2.0)
        self.cap_result = False
        
        buf = []
        try:
            while True:
                buf.append(self.angle_result.get_nowait())
        except:
            pass
        
        l_cam_angle,l_con_angle = 0.0, 0.0
        r_cam_angle,r_con_angle = 0.0, 0.0
        l = len(buf)
        for v in buf:
            l_cam_angle += v[0][0]
            l_con_angle += v[1][0]
            r_cam_angle += v[2][0]
            r_con_angle += v[3][0]
        l_cam_angle /= float(l)
        l_con_angle /= float(l)
        r_cam_angle /= float(l)
        r_con_angle /= float(l)
        
        l_cam_result = l_cam_angle <= self.max_lcambangle and l_cam_angle >= self.min_lcambangle
        l_con_result = l_con_angle <= self.max_lconsangle and l_con_angle >= self.min_lconsangle
        r_cam_result = r_cam_angle <= self.max_rcambangle and r_cam_angle >= self.min_rcambangle
        r_con_result = r_con_angle <= self.max_rconsangle and r_con_angle >= self.min_rconsangle
        
        return l_cam_angle, l_cam_result, l_con_angle, l_con_result, r_cam_angle, r_cam_result, r_con_angle, r_con_result
        
    def Sequnce(self, only_sensor = False):
        if self.ready:
            self.con.ShowBusyMessage(u'系统正在工作状态，按钮无效')
            return
        
        if app.condomain.IsScram():
            self.con.ShowBusyMessage(u'急停按钮被按下')
            return
        
        if not app.condomain.IsReady():
            self.con.ShowBusyMessage(u'工件没有放置就绪')
            return
        
        if app.condomain.IsManual():
            self.con.ShowBusyMessage(u'系统处于手动模式')
            return
        
        app.condomain.Green()
        try:
            app.condomain.Sequnce(only_sensor)
        except:
            self.con.ShowBusyMessage(u'运动出现异常，可能是传感器\n安装不到位或机构被卡住')
            return
        
        self.PartReady()
        
    def UnSequnce(self):
        if app.condomain.IsScram():
            self.con.ShowBusyMessage(u'急停按钮被按下')
            return
        
        if self.ready:
            if self.cap_result:
                return
            try:
                self.PartUnReady()
            except Exception,msg:
                self.con.ShowBusyMessage(unicode(msg))
                return
            except:
                self.con.ShowBusyMessage(u'处理结果出现异常，原因为知请报告错误情况')
                return
            
            try:
                app.condomain.UnSequnce()
            except:
                self.con.ShowBusyMessage(u'运动出现异常，可能是传感器安装不到位或机构被卡住')
                return
        else:
            app.condomain.Green()
            try:
                app.condomain.UnSequnce()
            except:
                self.con.ShowBusyMessage(u'运动出现异常，可能是传感器\n安装不到位或机构被卡住')
                return
        
    def OnStartButton(self):
        if app.condomain.IsManual():
            if app.condomain.IsScram():
                self.con.ShowBusyMessage(u'急停按钮被按下')
                return
            try:
                app.condomain.Sequnce(True)
            except:
                #self.con.ShowBusyMessage(u'Sequnce() 异常')
                self.con.ShowBusyMessage(u'运动出现异常，可能是传感器\n安装不到位或机构被卡住')
            return
        
        if not app.pref.no_barcode:
            self.con.ShowBusyMessage(u'系统处于条码模式，按钮无效')
            return
        
        if not self.last_barcode:
            self.con.ShowBusyMessage(u'没有扫描条码，请先扫描条码再启动')
            return
        
        self.Sequnce()
    
    def OnResetButton(self):
        self.con.HideMessageBox()
        self.UnSequnce()
        
    def OnPartReady(self):
        if not self.unready_sensor_flag:
            self.part_result = False
        else:
            self.unready_sensor_flag = False
        self.foc_result = []
        self.con.UpdateFocResult(None, None, None)
    
    def OnPartUnready(self):
        if self.CheckSensorPos():
            if not self.part_result:
                app.condomain.Red()
                self.con.ShowMessageBox(u"产品没有调试不合格")
        else:
            self.unready_sensor_flag = True
            
    def OnLeftSensorHome(self):
        self.CheckSensorPos()
    
    def OnRightSensorHome(self):
        self.CheckSensorPos()
    
    def CheckSensorPos(self):
        try:
            result = [i for i in range(len(self.pvs)) if self.pvs[i]>0]
            if len(result) > 0:
                infos = ["%d" % i for i in result]
                app.condomain.Red()
                self.con.ShowMessageBox(u"[%s]\n号传感器没到退到原位" % ", ".join(infos))
                return False
            return True
        except:
            error.traceback()
        
    def SaveResult(self, l_cam_angle, l_cam_result, l_con_angle, l_con_result, r_cam_angle, r_cam_result, r_con_angle, r_con_result, torque1, angle1, result1, torque2, angle2, result2, torque3, angle3, result3, torque4, angle4, result4):
        self.last_so_result = Database.MPart(parttype = self.last_so_parttype,
                                             barcode = self.last_barcode,
                                             machinebegintime = self.begin_time,
                                             machineendtime = self.end_time,
                                             lconsangle = l_con_angle,
                                             lconsresult = l_con_result and 'OK' or 'NOK',
                                             lcambangle = l_cam_angle,
                                             lcambresult = l_cam_result and 'OK' or 'NOK',
                                             rconsangle = r_con_angle,
                                             rconsresult = r_con_result and 'OK' or 'NOK',
                                             rcambangle = r_cam_angle,
                                             rcambresult = r_cam_result and 'OK' or 'NOK',
                                             torque1 = torque1,
                                             angle1 = angle1,
                                             result1 = result1,
                                             torque2 = torque2,
                                             angle2 = angle2,
                                             result2 = result2,
                                             torque3 = torque3,
                                             angle3 = angle3,
                                             result3 = result3,
                                             torque4 = torque4,
                                             angle4 = angle4,
                                             result4 = result4 )
        
def CalAngle(y1, x2, x3, y3, s1, s2, s3, s4, s5, s6):
        z = y1*((s3-s4)-(s1-s2))*x3 + (s1-s2)*x2*(y3-y1) + y1*(s1-s2)*x3 - y1*x2*((s5-s6)-(s1-s2))
        z = z/(x2*(y3-y1)+y1*x3)
        
        l_cam_angle = degrees(atan2(s1 - s2 - z, y1)) + 90.0
        l_con_angle = degrees(atan2(s3 - s4 - z, -x2)) + 90.0
        
        return l_cam_angle, l_con_angle, z
    
if __name__ == '__main__':
    print CalAngle(130.0, -130.0, 0.0, -130.0, 75,26.832,75,26.841,75,13.225)
    